//
// Created by geoffrey on 24-11-23.
//

#ifndef UUV_H
#define UUV_H

#include <iostream>
#include <iomanip>
#include <vector>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "Eular2Isometry.h"
#include <pangolin/pangolin.h>
#include "cross.h"
#include "MatlabEngine.hpp"
#include "MatlabDataArray.hpp"
using namespace Eigen;



class UUV
{
  private:

    double t=0;
//    Matrix<double, 6, 1>  x1;         //载体坐标系速度角速度
//    Matrix<double, 6, 1>  xEular;     //大地坐标系位姿(欧拉角)
    Matrix<double ,6 ,1> f = MatrixXd::Zero(6, 1);   //运载器所受克里奥利力和流体力
    Matrix<double, 6, 2>  state;
    vector<Matrix<double, 6, 2>,aligned_allocator<Matrix<double, 6, 2>>> State;
    vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> X_Isometry;
    double h=0.001 ;
    int i=0 ;

public:
    UUV();
    UUV(const Matrix<double, 6, 2> &_state);

    //获取时间
    double getT() const;
    //更新速度及位姿
    void update(Matrix<double, 6, 1> force);
    //获取速度及位姿矩阵
    vector<Matrix<double, 6, 2>,aligned_allocator<Matrix<double, 6, 2>>> getState() const;
    //获取速度及位姿
    Matrix<double, 6, 2> getstate() const;
    //获取运载器所受克里奥利力和流体力
    Matrix<double, 6, 1> getF() const;


};

Matrix<double, 6, 3> derivatives(double t , Matrix<double, 6, 2> state,const Matrix<double, 6, 1>& force);




#endif //UUV_H
